/**********************************************************************

  Audacity: A Digital Audio Editor

  Spectrum.cpp

  Dominic Mazzoni

**********************************************************************/

#include <math.h>
#include <stdio.h>

#include "Spectrum.h"
#include "FFT.h"

bool ComputeSpectrum(float mData[],int mDataLen,int mWindowSize,
					 float* processed[],int& mProcessedSize)
{
  if (mDataLen < mWindowSize) {
    // Not enough data to get even one window
    return false;
  }

   float *mProcessed = NULL;

   mProcessed = new float[mWindowSize];

   int i;
   for (i = 0; i < mWindowSize; i++)
      mProcessed[i] = 0.0;
   int half = mWindowSize / 2;

   float *in = new float[mWindowSize];
   float *in2 = new float[mWindowSize];
   float *out = new float[mWindowSize];
   float *out2 = new float[mWindowSize];

   int start = 0;
   int windows = 0;
   while (start + mWindowSize <= mDataLen) {
      // Copy stuff into in
      for (i = 0; i < mWindowSize; i++)
         in[i] = mData[start + i];

      // Window the data to lose crazy artifacts
      // due to finite-length window
      WindowFunc(2 /* Hamming */, mWindowSize, in);

      // Enhanced AC

      // Take FFT
      FFT(mWindowSize, false, in, NULL, out, out2);

      // Compute power
      for (i = 0; i < mWindowSize; i++)
        in[i] = (out[i] * out[i]) + (out2[i] * out2[i]);

      // Tolonen and Karjalainen recommend taking the cube root
      // of the power, instead of the square root

      for (i = 0; i < mWindowSize; i++)
        in[i] = pow(in[i], 1.0 / 3.0);

      // Take FFT
      FFT(mWindowSize, false, in, NULL, out, out2);

      // Take real part of result
      for (i = 0; i < half; i++)
        mProcessed[i] += out[i];

      start += half;
      windows++;
   }

   // Enhanced Autocorrelation
   for (i = 0; i < half; i++)
     mProcessed[i] = mProcessed[i] / windows;

   // Peak Pruning as described by Tolonen and Karjalainen, 2000

   // Clip at zero, copy to temp array
   for (i = 0; i < half; i++) {
     if (mProcessed[i] < 0.0)
       mProcessed[i] = 0.0;
     out[i] = mProcessed[i];
   }

   // Subtract a time-doubled signal (linearly interp.) from the original
   // (clipped) signal
   for (i = 0; i < half; i++)
     if ((i % 2) == 0)
       mProcessed[i] -= out[i / 2];
     else
       mProcessed[i] -= ((out[i / 2] + out[i / 2 + 1]) / 2);

   // Clip at zero again
   for (i = 0; i < half; i++)
     if (mProcessed[i] < 0.0)
       mProcessed[i] = 0.0;

   mProcessedSize = half;

   delete[]in;
   delete[]in2;
   delete[]out;
   delete[]out2;

   *processed = mProcessed;

   return true;
}

//from Audacity yin.c
float CubicMaximize(float y0, float y1, float y2, float y3, float *maxyVal)
{
   // Find coefficients of cubic
   float a, b, c, d;

   a = y0 / -6.0 + y1 / 2.0 - y2 / 2.0 + y3 / 6.0;
   b = y0 - 5.0 * y1 / 2.0 + 2.0 * y2 - y3 / 2.0;
   c = -11.0 * y0 / 6.0 + 3.0 * y1 - 3.0 * y2 / 2.0 + y3 / 3.0;
   d = y0;

   // Take derivative
   float da, db, dc;

   da = 3 * a;
   db = 2 * b;
   dc = c;

   // Find zeroes of derivative using quadratic equation
   float discriminant = db * db - 4 * da * dc;
   if (discriminant < 0.0)
      return -1.0;              // error

   float x1 = (-db + sqrt(discriminant)) / (2 * da);
   float x2 = (-db - sqrt(discriminant)) / (2 * da);

   // The one which corresponds to a local _maximum_ in the
   // cubic is the one we want - the one with a negative
   // second derivative
   float dda = 2 * da;
   float ddb = db;

#define CUBIC(x,a,b,c,d)  (a*x*x*x + b*x*x + c*x + d)

   if (dda * x1 + ddb < 0) {
     *maxyVal = CUBIC(x1,a,b,c,d);
     return x1;
   }
   else {
     *maxyVal = CUBIC(x2,a,b,c,d);
     return x2;
   }

#undef CUBIC
}

//from Audacity FreqWindow.cpp
float bestPeak(float *mProcessed, int mProcessedSize, float mRate)
{
   float highestpeak_y = 0;
   float highestpeak_x = 0;

   bool up = (mProcessed[1] > mProcessed[0]);
   for (int bin = 2; bin < mProcessedSize; bin++) {
     bool nowUp = mProcessed[bin] > mProcessed[bin - 1];
     if (!nowUp && up) {
       // Local maximum.  Find actual value by cubic interpolation
       int leftbin = bin - 2;
       if (leftbin < 1)
         leftbin = 1;

       float thispeak_y;
       float max = leftbin + CubicMaximize(mProcessed[leftbin],
                                           mProcessed[leftbin + 1],
                                           mProcessed[leftbin + 2],
                                           mProcessed[leftbin + 3],
                                           &thispeak_y);
       float thispeak_x = max;

       if (thispeak_y > highestpeak_y) {
         highestpeak_x = thispeak_x;
         highestpeak_y = thispeak_y;
       }
     }
	 up=nowUp;
   }
   return highestpeak_x;
}
